﻿using BZ.Core;
using NDK.Motion.Card;
using NDK.Motion.MachineResources;
using NDK.Motion.MiniEcatLib1;
using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Diagnostics;
using System.Linq;
using System.Threading;

namespace NDK.Motion.MachineManager
{
    public delegate void delegateShowMsgBox();

    public class MotionControl
    {
        private static log4net.ILog traceLogger = log4net.LogManager.GetLogger("MotionControl");
        private MotionControl()
        {
        }
        /// <summary>
        /// 窗体对象实例
        /// </summary>
        private static MotionControl _instance;
        private static readonly object singletonLock = new object();
        public static MotionControl Instance
        {
            get
            {
                if (_instance == null)
                {
                    lock (singletonLock)
                    {
                        if (_instance == null)
                        {
                            _instance = new MotionControl();
                        }
                    }
                }
                return _instance;
            }
        }
        private const int timeout = 20000;
        #region method
        public static event PropertyChangedEventHandler PropertyChanged;
        private static string status = string.Empty;
        private static string Status
        {
            get
            {
                return status;
            }
            set
            {
                status = value;
                PropertyChanged?.Invoke(Instance, new PropertyChangedEventArgs(value.ToString()));
            }
        }


        /// <summary>
        /// 根据回零优先级，回零所有轴：值越小优先级越高，优先级为负数，不参与回零
        /// </summary>
        /// <returns></returns>
        public static int HomeAllAxis()
        {
            int retCode = 0;
            int maxOrder = 0;
            List<Axis> axisList = new List<Axis>();
            Stopwatch sw = new Stopwatch();
            Status = "根据轴回零优先级，开始整机轴回零运动...";
            try
            {
                foreach (var axis in MachineDataManager.Instance.AxisList)
                {
                    if (axis.IsEnableHome)
                    {
                        if (axis.HomeOrder > maxOrder)//得到优先级最大的值
                        {
                            maxOrder = axis.HomeOrder;
                        }
                        if (axis.HomeOrder >= 0)//得到需要回零的轴
                        {
                            axisList.Add(axis);
                        }
                    }
                }
                for (int i = 0; i <= maxOrder; i++)//按优先级回零
                {
                    List<Axis> tempAxisList = axisList.FindAll(a => a.HomeOrder == i);
                    if (tempAxisList?.Count == 0 || tempAxisList == null)
                    {
                        continue;
                    }
                    Status = $"优先级为{i}，轴开始回零运动...";

                    foreach (var axis in tempAxisList)
                    {
                        axis.IsStop = false;
                        Status = $"轴：{axis.Name}，轴号：{axis.AxisID}，开始回零运动...";
                        sw.Restart();
                        if (axis.HomeMove() == false)//有一个轴失败，就返回失败
                        {
                            Status = $"轴：{axis.Name}，轴号：{axis.AxisID}，回零失败！耗时：{sw.ElapsedMilliseconds}ms";
                            StopMoveAll(STOP_TYPE.EMG_STOP);
                            return 4;
                        }
                    }
                    foreach (var axis in tempAxisList)
                    {
                        if (tempAxisList?.Count == 0 || tempAxisList == null)
                        {
                            continue;
                        }
                        sw.Restart();
                        retCode = axis.WaitHomeMoveDone(30000);
                        foreach (Axis axis1 in MachineDataManager.Instance.AxisList)//恢复轴的外部触发停止状态
                        {
                            axis1.IsStop = false;
                        }
                        if (retCode == 1)//超时
                        {
                            Status = $"等待轴：{axis.Name}，轴号：{axis.AxisID}，回零超时！耗时：{sw.ElapsedMilliseconds}ms";
                            StopMoveAll(STOP_TYPE.EMG_STOP);
                            return 1;
                        }
                        if (retCode == 2)//暂停
                        {
                            Status = $"等待轴：{axis.Name}，轴号：{axis.AxisID}，回零时，被外部暂停！耗时：{sw.ElapsedMilliseconds}ms";
                            StopMoveAll(STOP_TYPE.EMG_STOP);
                            return 2;
                        }

                        if (retCode == 3)// 
                        {
                            Status = $"轴：{axis.Name}，轴号：{axis.AxisID}，回零时掉使能：{sw.ElapsedMilliseconds}ms";
                            return 3;
                        }

                        Status = $"轴：{axis.Name}，轴号：{axis.AxisID}，回零时完成！耗时：{sw.ElapsedMilliseconds}ms";
                    }
                }
            }
            catch (Exception ex)
            {
                traceLogger.Error("整机回零异常！" + ex.Message + ex.StackTrace);
                return 4;
            }
            return 0;
        }


        /// <summary>
        /// 走点位，不等待运动结束，忽略优先级
        /// </summary>
        /// <param name="pos"></param>
        public static bool AxesMoveNoWait(Position pos)
        {
            try
            {
                //得到该点位的复制点，避免运动过程中，该对象在外部被修改,但是轴需要用外部对象，实现暂停功能
                Position position = pos.Clone();

                //判断轴的个数
                int axisTotalNum = position.AxisList.Count;
                Dictionary<string, List<Axis>> keyValuePairs = new Dictionary<string, List<Axis>>
                {
                    { "1", new List<Axis>() },
                    { "2", new List<Axis>() },
                    { "3", new List<Axis>() },
                    { "4", new List<Axis>() },
                    { "5", new List<Axis>() },
                    { "6", new List<Axis>() },
                    { "7", new List<Axis>() },
                    { "8", new List<Axis>() },
                    { "9", new List<Axis>() }
                };

                List<Axis> posAxisList = new List<Axis>();
                foreach (var posAxis in pos.AxisList)
                {
                    VelocityProfile vel = position.VelocityProfiles.SingleOrDefault(v => v.AxisName == posAxis.Name);
                    //检查轴是否屏蔽了，不需要运动
                    if (vel.Shield == false)
                    {
                        posAxisList.Add(posAxis);
                    }

                    if (pos.IsAllowUniform)
                    {
                        vel.MoveVel *= (MachineDataManager.Instance.MotionSpeedPercentage / 100);
                        vel.Acc *= (MachineDataManager.Instance.MotionSpeedPercentage / 100);
                        vel.Dec *= (MachineDataManager.Instance.MotionSpeedPercentage / 100);
                    }
                }

                //将轴进行优先级排序
                for (int axisIndex = 0; axisIndex < position.AxisPriority.Length; axisIndex++)
                {
                    string priority = position.AxisPriority.Substring(axisIndex, 1);
                    Axis axis = MachineDataManager.Instance.AxisList.SingleOrDefault(a => a.AxisID == posAxisList[axisIndex].AxisID && a.Name == posAxisList[axisIndex].Name);
                    if (priority != "0")
                    {
                        keyValuePairs[priority].Add(axis);
                    }
                }

                //按照优先级顺序，进行运动控制
                foreach (var priorityAxisList in keyValuePairs.Values)
                {
                    foreach (var axis in priorityAxisList)
                    {
                        VelocityProfile vel = position.VelocityProfiles.SingleOrDefault(v => v.AxisName == axis.Name);
                        //检查轴是否屏蔽了，不需要运动
                        if (vel.Shield == false)
                        {
                            bool ret = axis.MoveAbs(vel);
                            if (!ret)
                            {
                                Status = $"函数执行错误，轴号:  {axis.AxisID}   轴名称：{axis.Name}     当前点位: {axis.GetPos()}  设定点位: {vel.Pos}";
                                traceLogger.Error("函数执行错误！" + Status);
                            }
                        }
                    }
                }
            }
            catch (Exception ex)
            {
                traceLogger.Error(ex.StackTrace);
                return false;
            }
            return true;
        }

        public class LineInterpolationParam
        {
            public Axis axis;
            public VelocityProfile velocityProfile;
        }

        public static List<VelocityProfile> getVpList(List<Position> posList)
        {
            if (posList == null)
                return null;
            List<Position> positionList = new List<Position>();
            foreach (var pos in posList)
            {
                positionList.Add(pos.Clone());
            }

            List<VelocityProfile> vpList = new List<VelocityProfile>();
            foreach (var pos in positionList)
            {
                foreach (var vp in pos.VelocityProfiles)
                {
                    if (vp.Shield == false)
                    {
                        vpList.Add(vp);

                        if (pos.IsAllowUniform)
                        {
                            vp.MoveVel = Math.Ceiling(vp.MoveVel *= (MachineDataManager.Instance.MotionSpeedPercentage / 100));
                            vp.Acc = Math.Ceiling(vp.Acc *= (MachineDataManager.Instance.MotionSpeedPercentage / 100));
                            vp.Dec = Math.Ceiling(vp.Dec *= (MachineDataManager.Instance.MotionSpeedPercentage / 100));
                        }
                    }


                }
            }
            return vpList;
        }
        public static bool LineInterpolationMove(List<Position> posList, int timeout = timeout)
        {
            if (posList == null)
                return false;
            List<Position> positionList = new List<Position>();
            foreach (var pos in posList)
            {
                positionList.Add(pos.Clone());
            }

            List<VelocityProfile> vpList = new List<VelocityProfile>();
            foreach (var pos in positionList)
            {
                foreach (var vp in pos.VelocityProfiles)
                {
                    if (vp.Shield == false)
                    {
                        vpList.Add(vp);

                        if (pos.IsAllowUniform)
                        {
                            vp.MoveVel = Math.Ceiling(vp.MoveVel *= (MachineDataManager.Instance.MotionSpeedPercentage / 100));
                            vp.Acc = Math.Ceiling(vp.Acc *= (MachineDataManager.Instance.MotionSpeedPercentage / 100));
                            vp.Dec = Math.Ceiling(vp.Dec *= (MachineDataManager.Instance.MotionSpeedPercentage / 100));
                        }
                    }

                }
            }

            List<LineInterpolationParam> lineInterpolationParamList = new List<LineInterpolationParam>();
            foreach (var vp in vpList)
            {
                Axis axis = MachineDataManager.Instance.AxisList.SingleOrDefault(a => a.AxisID == vp.AxisId);
                if (axis != null)
                {
                    LineInterpolationParam lineInterpolationParam = new LineInterpolationParam();
                    lineInterpolationParam.axis = axis;
                    lineInterpolationParam.velocityProfile = vp;
                    lineInterpolationParamList.Add(lineInterpolationParam);
                }
            }
            try
            {
                LineInterpolation(lineInterpolationParamList);


                if (WaitMoveDone(vpList, timeout) == false)
                    return false;

            }
            catch (Exception ex)
            {
                return false;
            }

            return true;
        }

        public static bool LineInterpolationMoveNoWait(List<VelocityProfile> vpList)
        {
            List<LineInterpolationParam> lineInterpolationParamList = new List<LineInterpolationParam>();
            foreach (var vp in vpList)
            {
                Axis axis = MachineDataManager.Instance.AxisList.SingleOrDefault(a => a.AxisID == vp.AxisId);
                if (axis != null)
                {
                    LineInterpolationParam lineInterpolationParam = new LineInterpolationParam();
                    lineInterpolationParam.axis = axis;
                    lineInterpolationParam.velocityProfile = vp;
                    lineInterpolationParamList.Add(lineInterpolationParam);
                }
            }
            try
            {
                LineInterpolation(lineInterpolationParamList);
            }
            catch (Exception ex)
            {
                return false;
            }

            return true;
        }
        public static bool LineInterpolation(List<LineInterpolationParam> lineInterpolationParamList)
        {
            if (lineInterpolationParamList == null || lineInterpolationParamList.Count < 2 || lineInterpolationParamList.Count > 6)
                return false;

            Axis axisMain = lineInterpolationParamList[0].axis;
            if (axisMain.Card.Initialized == false)
            {
                throw new AxisException($"{axisMain.Card.CardType} Card Is NotInitialized");
            }
            try
            {
                if (axisMain.IsStop == true)
                {
                    return false;
                }

                double vm = 0;
                double acc = 0;

                List<int> axisIdList = new List<int>();
                List<double> posList = new List<double>();
                foreach (var lineInterpolationParam in lineInterpolationParamList)
                {
                    Axis axis = lineInterpolationParam.axis;
                    VelocityProfile vp = lineInterpolationParam.velocityProfile;
                    if (((vp.Pos > axis.SPel) || (vp.Pos < axis.SMel)) && axis.IsEnableSoftLimit)
                    {
                        throw new AxisException("Axis[" + axis.AxisID + "]:MoveAbs Error!" + "目标位置:" + vp.Pos.ToString() + "超限");
                    }
                    if (axis.MaxVel <= vp.MoveVel)
                    {
                        vp.MoveVel = axis.MaxVel;
                    }
                    if (vm == 0)
                        vm = vp.MoveVel * axis.Ratio;
                    else if (vm > vp.MoveVel * axis.Ratio)
                        vm = vp.MoveVel * axis.Ratio;
                    if (acc == 0)
                        acc = vp.Acc * axis.Ratio;
                    else if (vm > vp.Acc * axis.Ratio)
                        acc = vp.Acc * axis.Ratio;

                    axisIdList.Add(vp.AxisId);
                    posList.Add(vp.Pos * axis.Ratio);
                }

                return axisMain.Card.InterPolationLine(axisIdList.ToArray(), posList.ToArray(), PRA_OPTION_ENUM.Absolute, vm, acc);
                //return true;
            }
            catch (Exception ex)
            {
                throw new AxisException("StartGear Exception! " + ex.Message + ex.StackTrace);
            }
        }

        public static bool WaitMoveDone(List<VelocityProfile> vpList, int timeout = timeout)
        {
            try
            {
                if (WaitingMoveDone(vpList, timeout) == false)
                    return false;
            }
            catch (Exception ex)
            {
                return false;
            }
            return true;
        }
        public static bool AxesMove(Position pos, int timeout = timeout)
        {
            //得到该点位的复制点，避免运动过程中，该对象在外部被修改,但是轴需要用外部对象，实现暂停功能
            Position position = pos.Clone();

            //判断轴的个数
            int axisTotalNum = position.AxisList.Count;
            Dictionary<string, List<Axis>> keyValuePairs = new Dictionary<string, List<Axis>>
                {
                    { "1", new List<Axis>() },
                    { "2", new List<Axis>() },
                    { "3", new List<Axis>() },
                    { "4", new List<Axis>() },
                    { "5", new List<Axis>() },
                    { "6", new List<Axis>() },
                    { "7", new List<Axis>() },
                    { "8", new List<Axis>() },
                    { "9", new List<Axis>() }
                };

            List<Axis> posAxisList = new List<Axis>();
            foreach (var posAxis in pos.AxisList)
            {
                VelocityProfile vel = position.VelocityProfiles.SingleOrDefault(v => v.AxisName == posAxis.Name);
                //检查轴是否屏蔽了，不需要运动
                if (vel.Shield == false)
                {
                    posAxisList.Add(posAxis);
                }

                if (pos.IsAllowUniform)
                {
                    vel.MoveVel *= (MachineDataManager.Instance.MotionSpeedPercentage / 100);
                    vel.Acc *= (MachineDataManager.Instance.MotionSpeedPercentage / 100);
                    vel.Dec *= (MachineDataManager.Instance.MotionSpeedPercentage / 100);
                }
            }

            //将轴进行优先级排序
            for (int axisIndex = 0; axisIndex < position.AxisPriority.Length; axisIndex++)
            {
                string priority = position.AxisPriority.Substring(axisIndex, 1);
                Axis axis = MachineDataManager.Instance.AxisList.SingleOrDefault(a => a.AxisID == posAxisList[axisIndex].AxisID && a.Name == posAxisList[axisIndex].Name);
                if (priority != "0")
                {
                    keyValuePairs[priority].Add(axis);
                }
            }

            //按照优先级顺序，进行运动控制
            foreach (var priorityAxisList in keyValuePairs.Values)
            {
                foreach (var axis in priorityAxisList)
                {
                    VelocityProfile vel = position.VelocityProfiles.SingleOrDefault(v => v.AxisName == axis.Name);
                    //检查轴是否屏蔽了，不需要运动
                    if (vel.Shield == false)
                    {
                        bool ret = axis.MoveAbs(vel);
                        if (!ret)
                        {
                            Status = $"函数执行错误，轴号:  {axis.AxisID}   轴名称：{axis.Name}     当前点位: {axis.GetPos()}  设定点位: {vel.Pos}";
                            traceLogger.Error("函数执行错误！" + Status);
                        }
                    }
                }
                if (WaitingMoveDone(priorityAxisList, position, timeout) == false) return false;
            }
            return true;
        }

        private static bool WaitingMoveDone(List<Axis> axisList, Position position, int timeout = timeout)
        {
            if (axisList == null || axisList.Count == 0)
            {
                //Status = $"WaitingMoveDone Exception:axisList is Null or axisList Count Is 0!";
                //throw new AxisException($"WaitingMoveDone Exception:axisList is Null or axisList Count Is 0!");
                return true;
            }
            foreach (var axis in axisList)
            {
                //检查轴是否屏蔽了，不需要运动
                VelocityProfile vel = position.VelocityProfiles.SingleOrDefault(v => v.AxisName == axis.Name);
                if (vel.Shield == true)
                {
                    continue;
                }
                int retCode = axis.WaitMoveDone(vel.Pos, timeout);
                if (retCode == 2)//外部触发停止（暂停）
                {
                    Status = $"等待轴{axis.Name},轴号{axis.AxisID}运动完成时,被外部触发停止";
                    foreach (var a in MachineDataManager.Instance.AxisList) a.StopMoveEmg();

                    return false;
                }
                if (retCode == 3)//掉使能停止
                {
                    Status = $"等待轴{axis.Name},轴号{axis.AxisID}运动完成时,掉使能！";
                    throw new AxisException($"轴号:{axis.AxisID}, 轴名称:{axis.Name}   ,掉使能!");
                }
                if (retCode == 1)//超时停止
                {
                    Status = $"等到轴到位超时，轴号:  {axis.AxisID}   轴名称：{axis.Name}     当前点位: {axis.GetPos()}  设定点位: {vel.Pos}";
                    throw new AxisException($"等到轴到位超时，轴号:  {axis.AxisID}   轴名称：{axis.Name}     当前点位: {axis.GetPos()}  设定点位: {vel.Pos}");
                }
                if (retCode == 0)//运动完成
                {
                    double CmdPos = axis.GetCmdPos();
                    double TargetPos = vel.Pos;
                    double errPos = Math.Abs((TargetPos - CmdPos));
                    //double errPos = axis.GetErrPos();
                    if (Math.Abs(errPos) > axis.ErrBand)
                    {
                        Status = $"轴到位误差过大，轴号:  {axis.AxisID}   轴名称：{axis.Name}    误差为：{errPos}超过设定的误差阈值（{axis.ErrBand}";
                        throw new AxisException($"轴到位误差过大，轴号:  {axis.AxisID}   轴名称：{axis.Name}    误差为：{errPos}超过设定的误差阈值（{axis.ErrBand}");
                    }
                }
            }
            return true;
        }


        private static bool WaitingMoveDone(List<VelocityProfile> vpList, int timeout = timeout)
        {
            if (vpList == null || vpList.Count == 0)
            {
                //Status = $"WaitingMoveDone Exception:axisList is Null or axisList Count Is 0!";
                //throw new AxisException($"WaitingMoveDone Exception:axisList is Null or axisList Count Is 0!");
                return true;
            }
            foreach (var vp in vpList)
            {
                if (vp.Shield == true)
                {
                    continue;
                }
                Axis axis = MachineDataManager.Instance.AxisList.SingleOrDefault(a => a.AxisID == vp.AxisId);
                if (axis == null)
                {
                    return true;
                }
                int retCode = axis.WaitMoveDone(vp.Pos, timeout);
                if (retCode == 2)//外部触发停止（暂停）
                {
                    Status = $"等待轴{axis.Name},轴号{axis.AxisID}运动完成时,被外部触发停止";
                    foreach (var a in MachineDataManager.Instance.AxisList) a.StopMoveEmg();

                    return false;
                }
                if (retCode == 3)//掉使能停止
                {
                    Status = $"等待轴{axis.Name},轴号{axis.AxisID}运动完成时,掉使能！";
                    throw new AxisException($"轴号:{axis.AxisID}, 轴名称:{axis.Name}   ,掉使能!");
                }
                if (retCode == 1)//超时停止
                {
                    Status = $"等到轴到位超时，轴号:  {axis.AxisID}   轴名称：{axis.Name}     当前点位: {axis.GetPos()}  设定点位: {vp.Pos}";
                    throw new AxisException($"等到轴到位超时，轴号:  {axis.AxisID}   轴名称：{axis.Name}     当前点位: {axis.GetPos()}  设定点位: {vp.Pos}");
                }
                if (retCode == 0)//运动完成
                {
                    //double errPos = axis.GetErrPos();
                    double CmdPos = axis.GetCmdPos();
                    double TargetPos = vp.Pos;
                    double errPos = Math.Abs((TargetPos - CmdPos));
                    if (errPos > axis.ErrBand)
                    {
                        Status = $"轴到位误差过大，轴号:  {axis.AxisID}   轴名称：{axis.Name}    误差为：{errPos}超过设定的误差阈值（{axis.ErrBand}";
                        throw new AxisException($"轴到位误差过大，轴号:  {axis.AxisID}   轴名称：{axis.Name}    误差为：{errPos}超过设定的误差阈值（{axis.ErrBand}");
                    }
                }
            }
            return true;
        }
        public static bool WaitingMoveDone(Position position, int timeout = timeout)
        {
            foreach (var axis in position.AxisList)
            {
                //检查轴是否屏蔽了，不需要运动
                VelocityProfile vel = position.VelocityProfiles.SingleOrDefault(v => v.AxisName == axis.Name);
                if (vel.Shield == true)
                {
                    continue;
                }
                Axis axis1 = MachineDataManager.Instance.AxisList.SingleOrDefault(a => a.AxisID == axis.AxisID && a.Name == axis.Name);
                int retCode = axis1.WaitMoveDone(vel.Pos);
                if (retCode == 2)//外部触发停止（暂停）
                {
                    Status = $"等待轴{axis.Name},轴号{axis.AxisID}运动完成时,被外部触发停止";
                    foreach (var a in MachineDataManager.Instance.AxisList) a.StopMoveEmg();

                    return false;
                }
                if (retCode == 3)//掉使能停止
                {
                    Status = $"等待轴{axis.Name},轴号{axis.AxisID}运动完成时,掉使能！";
                    throw new AxisException($"轴号:{axis1.AxisID}, 轴名称:{axis1.Name}   ,掉使能!");
                }
                if (retCode == 1)//超时停止
                {
                    Status = $"等到轴到位超时，轴号:  {axis.AxisID}   轴名称：{axis.Name}     当前点位: {axis.GetPos()}  设定点位: {vel.Pos}";
                    throw new AxisException($"等到轴到位超时，轴号:  {axis1.AxisID}   轴名称：{axis1.Name}     当前点位: {axis1.GetPos()}  设定点位: {vel.Pos}");
                }
                if (retCode == 0)//运动完成
                {
                    //double errPos = axis1.GetErrPos();
                    double CmdPos = axis.GetCmdPos();
                    double TargetPos = vel.Pos;
                    double errPos = Math.Abs((TargetPos - CmdPos));
                    if (Math.Abs(errPos) > axis1.ErrBand)
                    {
                        Status = $"轴到位误差过大，轴号:  {axis.AxisID}   轴名称：{axis.Name}    误差为：{errPos}超过设定的误差阈值（{axis.ErrBand}";
                        throw new AxisException($"轴到位误差过大，轴号:  {axis1.AxisID}   轴名称：{axis1.Name}    误差为：{errPos}超过设定的误差阈值（{axis1.ErrBand}");
                    }
                    return true;
                }
            }
            return true;
        }


        /// <summary>
        /// 停止所有轴
        /// </summary>
        /// <param name="stopType"></param>
        public static void StopMoveAll(STOP_TYPE stopType)
        {
            switch (stopType)
            {
                case STOP_TYPE.DEC_STOP:
                    foreach (var axis in MachineDataManager.Instance.AxisList)
                    {
                        axis.StopMoveDec();
                    }
                    break;
                case STOP_TYPE.EMG_STOP:
                    foreach (var axis in MachineDataManager.Instance.AxisList)
                    {
                        axis.StopMoveEmg();
                    }
                    break;
                default:
                    break;
            }
        }


        /// <summary>
        /// 停止指定点位的运动
        /// </summary>
        /// <param name="pointPosition"></param>
        /// <returns></returns>
        public static bool AxesMoveStop(Position pos)
        {
            try
            {
                Position position = pos.Clone();
                foreach (var axis in position.AxisList)
                {
                    Axis axis1 = MachineDataManager.Instance.AxisList.SingleOrDefault(a => a.AxisID == axis.AxisID && a.Name == axis.Name);
                    axis1.StopMoveEmg();
                }
            }
            catch (Exception ex)
            {
                throw new AxisException($"AxesMoveStop Exception!{ex.Message + ex.StackTrace}");
            }
            return true;
        }
        /// <summary>
        /// 检测该点位的所有轴是否停止运动
        /// </summary>
        /// <param name="pointPosition"></param>
        /// <param name="pra"></param>
        /// <returns></returns>
        public static bool AxesIsMoving(Position pos)
        {
            try
            {
                Position position = pos.Clone();
                foreach (var axis in position.AxisList)
                {
                    Axis axis1 = MachineDataManager.Instance.AxisList.SingleOrDefault(a => a.AxisID == axis.AxisID && a.Name == axis.Name);

                    if (axis1.IsMoving() == true)
                    {
                        return true;
                    }
                }
            }
            catch (Exception ex)
            {
                throw new AxisException($"AxesIsMoving Exception!{ex.Message + ex.StackTrace}");
            }

            return false;
        }


        /// <summary>
        /// 失能所有轴
        /// </summary>
        public static bool AllAxisServoOff()
        {
            try
            {
                var orderedAxisList = MachineDataManager.Instance.AxisList.OrderBy(r => r.ServoOffOrder).Select(r => r).ToList();

                foreach (var axis in orderedAxisList)
                {
                    if (axis.ServoOff() == false)
                    {
                        return false;
                    }
                    Thread.Sleep(100);
                }
            }
            catch (Exception ex)
            {
                throw new AxisException($"All Servo OFF, Exception Message:{ex.Message}, Exception StackTrace:{ex.StackTrace}！");
            }
            return true;
        }
        /// <summary>
        /// 使能所有轴
        /// </summary>
        public static RES AllAxisServoON()
        {
            RES res = new RES();
            try
            {
                var orderedAxisList = MachineDataManager.Instance.AxisList.OrderBy(r => r.ServoOffOrder).Select(r => r).ToList();

                string NotSerOnAxisStr = "";
                bool isAllServoOn = true;
                foreach (var axis in orderedAxisList)
                {
                    if (axis.ServoOn() == false)
                    {
                        isAllServoOn = false;
                        NotSerOnAxisStr += $"{axis.AxisID}-{axis.Name},";
                    }
                    Thread.Sleep(100);
                }
                if (!isAllServoOn)
                {
                    res.bRtn = false;
                    res.msg = NotSerOnAxisStr;
                }
            }
            catch (Exception ex)
            {
                throw new AxisException($"All Servo ON, Exception Message:{ex.Message}, Exception StackTrace:{ex.StackTrace}！");
            }
            res.bRtn = true;
            return res;
        }

        /// <summary>
        /// 断开使能所有轴
        /// </summary>
        public static RES AllAxisServoOFF()
        {
            RES res = new RES();
            try
            {
                var orderedAxisList = MachineDataManager.Instance.AxisList.OrderBy(r => r.ServoOffOrder).Select(r => r).ToList();

                string NotSerOffAxisStr = "";
                bool isAllServoOff = true;
                foreach (var axis in orderedAxisList)
                {
                    if (axis.ServoOff() == false)
                    {
                        isAllServoOff = false;
                        NotSerOffAxisStr += $"{axis.AxisID}-{axis.Name},";
                    }
                    Thread.Sleep(100);
                }
                if (!isAllServoOff)
                {
                    res.bRtn = false;
                    res.msg = NotSerOffAxisStr;
                }
            }
            catch (Exception ex)
            {
                throw new AxisException($"All Servo OFF, Exception Message:{ex.Message}, Exception StackTrace:{ex.StackTrace}！");
            }
            res.bRtn = true;
            return res;
        }

        static Position GetEndPositionByScanLength(InterpolationParam param, bool UseOffeset = false)
        {
            // 判断方向
            var pos = param.EndPosiiton.Clone();
            if (!UseOffeset)
                pos.SetPos(param.axisName, param.StartPosiiton.GetPos(param.axisName) > param.EndPosiiton.GetPos(param.axisName) ? Math.Abs(param.length)
                                                : -Math.Abs(param.length));
            else
                pos.SetPos(param.axisName, param.StartPosiiton.GetPos(param.axisName) > param.EndPosiiton.GetPos(param.axisName) ? -(Math.Abs(param.length) + 0.3)
                                        : Math.Abs(param.length) + 0.3);
            return pos;
        }

        public static bool InterpolationMotion(List<InterpolationParam> Params)
        {
            try
            {
                var paramters = new List<M60InterpolationMotionParamter>();
                foreach (var item in Params)
                {
                    switch (item.SelectType)
                    {
                        case 1:
                            paramters.Add(new M60Line(item.StartPosiiton));
                            break;
                        case 2:
                            List<M60BufIos.M60BufIOItem> m60BufIOItemON = new List<M60BufIos.M60BufIOItem>();
                            foreach (var item2 in item.Outputs)
                            {
                                if (item2.Name.Contains("上激光"))
                                    m60BufIOItemON.Add(new M60BufIos.M60BufIOItem(item2, false));
                                else
                                    m60BufIOItemON.Add(new M60BufIos.M60BufIOItem(item2, true));
                            }
                            paramters.Add(new M60BufIos(m60BufIOItemON));
                            break;
                        case 3:
                            paramters.Add(new M60Line(GetEndPositionByScanLength(item)));
                            break;
                        case 4:
                            List<M60BufIos.M60BufIOItem> m60BufIOItemOFF = new List<M60BufIos.M60BufIOItem>();
                            foreach (var item2 in item.Outputs)
                            {
                                if (item2.Name.Contains("上激光"))
                                    m60BufIOItemOFF.Add(new M60BufIos.M60BufIOItem(item2, true));
                                else
                                    m60BufIOItemOFF.Add(new M60BufIos.M60BufIOItem(item2, false));
                            }
                            paramters.Add(new M60BufIos(m60BufIOItemOFF));
                            break;
                        case 5:
                            paramters.Add(new M60Line(GetEndPositionByScanLength(item, true)));
                            break;
                        case 6:
                            paramters.Add(new M60Delay(item.delay));
                            break;
                    }
                }
                var m60 = new M60InterpolationMotion();
                m60.Initial(1, paramters.ToArray());
                return m60.Move();
            }
            catch (Exception)
            {
                return false;
            }

        }

        #region 凌华/凌臣飞拍模块
        /// <summary>
        /// 
        /// </summary>
        /// <param name="path">Mini文件加载</param>
        /// <param name="MiniCount">Mini模块个数</param>
        /// <returns></returns>
        public static bool IniMiniEcatLib(string path, int MiniCount)
        {
            int num = 0;
            MiniEcatLib.Mb_InitEcat(ref num, 0);
            if (num == MiniCount)
            {
                num = MiniEcatLib.Mb_LoadParamFromFile(path);
                if (num != 0)
                    return false;
            }
            else
            {
                return false;
            }
            return true;
        }

        /// <summary>
        /// 飞拍
        /// </summary>
        /// <param name="PreCompareNo">比较器数量0~3</param>
        /// <param name="pos">压入点位</param>
        /// <param name="EncoderData">当前起始位编码器值</param>
        /// <param name="SlaveNo">从站号</param>
        /// <param name="EncoderNo">编码器编号0~3</param>
        /// <returns></returns>
        public static bool CMP_Air(int[] PreCompareNo, int[] pos, int EncoderData, int SlaveNo = 1, int EncoderNo = 0)
        {
            int ret = 0;
            for (int i = 0; i < PreCompareNo.Length; i++)
            {
                ret += MiniEcatLib.Mb_E4O4PreCmp_SetEnable(SlaveNo, PreCompareNo[i], 0); //关闭
                ret += MiniEcatLib.Mb_E4O4PreCmp_ResetTrigData(SlaveNo, PreCompareNo[i]);//重置压入点位
            }
                
            ret += MiniEcatLib.Mb_E4O4Encoder_SetCurrentData(SlaveNo, EncoderNo, EncoderData);//设置编码器
            for (int i = 0; i < PreCompareNo.Length; i++)
            {
                ret += MiniEcatLib.Mb_E4O4PreCmp_SetTrigData(SlaveNo, PreCompareNo[i], ref pos[0], pos.Length); //压入点位
                int Level = 0;
                ret += MiniEcatLib.Mb_E4O4TrigOut_GetCurrentTrigLevel(SlaveNo, PreCompareNo[i], ref Level);   //判断高低电平
                if (Level != 0)
                {
                    ret += MiniEcatLib.Mb_E4O4TrigOut_SetManualPulseOutput(SlaveNo, Convert.ToInt32(Math.Pow(2, PreCompareNo[i])));
                }
                ret += MiniEcatLib.Mb_E4O4PreCmp_SetEnable(SlaveNo, PreCompareNo[i], 0); //关闭
                ret += MiniEcatLib.Mb_E4O4PreCmp_SetEnable(SlaveNo, PreCompareNo[i], 1); //开启
            }

            return ret == 0;
        }


        /// <summary>
        /// 设置指定通道的编码器计数值
        /// </summary>
        /// <param name="EncoderNo">编码器号编码器通道号，0~3</param>
        /// <param name="EncoderData">数据设置编码器当前的数据</param>
        /// <param name="SlaveNo">从站号，若不开启别名则从1开始计数</param>
        /// <returns></returns>
        public static bool SetCurrentData(int EncoderNo, int EncoderData, int SlaveNo = 1)
        {
            int ret = 0;
            ret = MiniEcatLib.Mb_E4O4Encoder_SetCurrentData(SlaveNo, EncoderNo, EncoderData);
            if (ret != 0)
                return false;
            return true;
        }

        /// <summary>
        /// 设置预设定比较器的触发点位
        /// </summary>
        /// <param name="PreCompareNo">预设定比较器号0-3</param>
        /// <param name="ints">触发点位位置数组，数据数量1-1023</param>
        /// <param name="SlaveNo"></param>
        /// <returns></returns>
        public static bool SetTrigData(int PreCompareNo, int[] ints, int SlaveNo = 1)
        {
            int ret = 0;
            ret = MiniEcatLib.Mb_E4O4PreCmp_SetTrigData(SlaveNo, PreCompareNo, ref ints[0], ints.Length);
            if (ret != 0)
                return false;
            return true;
        }

        /// <summary>
        /// 设置预设定比较器使能状态
        /// </summary>
        /// <param name="PreCompareNo"></param>
        /// <param name="Enable">打开需要运行两次,先0后1</param>
        /// <param name="SlaveNo"></param>
        /// <returns></returns>
        public static bool SetEnable(int PreCompareNo, int Enable = 0, int SlaveNo = 1)
        {
            int ret = 0;
            ret = MiniEcatLib.Mb_E4O4PreCmp_SetEnable(SlaveNo, PreCompareNo, Enable);
            if (ret != 0)
                return false;
            return true;
        }

        /// <summary>
        /// 重置预设定比较器FIFO中的触发数据
        /// </summary>
        /// <param name="PreCompareNo"></param>
        /// <param name="SlaveNo"></param>
        /// <returns></returns>
        public static bool ResetTrigData(int PreCompareNo, int SlaveNo = 1)
        {
            int ret = 0;
            ret = MiniEcatLib.Mb_E4O4PreCmp_ResetTrigData(SlaveNo, PreCompareNo);
            if (ret != 0)
                return false;
            return true;
        }

        #endregion


        #endregion

        public class RES
        {
            public bool bRtn { get; set; } = true;
            public string msg { get; set; } = "";
        }

        [System.Runtime.InteropServices.DllImport("winmm")]
        private static extern int timeGetTime();

        /// <summary>
        /// 延时，单位ms
        /// </summary>
        /// <param name="milliSeconds"></param>
        private static void Delay(int milliSeconds)
        {
            try
            {
                long interval;
                int oldTime = timeGetTime();
                do
                {
                    System.Windows.Forms.Application.DoEvents();
                    interval = timeGetTime() - oldTime;
                    if (interval < 0) interval = (long)Math.Pow(2, 32) - Math.Abs(interval);
                } while (interval < milliSeconds);
            }
            catch { }
        }
    }
    public class InterpolationParam
    {
        public Position StartPosiiton;
        public Position EndPosiiton;
        public List<Axis> Axes;
        public List<OutputSetting> Outputs;
        public double length;
        public string axisName;
        public int SelectType;
        public int delay;
    }
}
